#import ctypes.wintypes
#import email_interface
import inspect
import os
import pywintypes
#import win32api
import win32com.client as com
#import shutil
import numpy as np
import Corflo_RM as RM
import random
#import fzpEvaluation as fzp
#import win_interface

def getProgramName():
    PROGRAM_FULL_NAME = inspect.getfile(inspect.currentframe())
    PROGRAM_NAME = os.path.split(PROGRAM_FULL_NAME)[-1]
    return PROGRAM_NAME

def mkdir(path):
    ''' recursively create given path '''
    upper, name = os.path.split(path)
    if not os.path.exists(upper):
        mkdir(upper)
    # if path already exists, do not create. Otherwise, error will occur
    if not os.path.exists(path):
        os.mkdir(path)  


def calcRandomSeeds(nMonteCarlo):
    return [3*i + 1 for i in range(nMonteCarlo)]
    

def writeHeader(fileName, title, Nsec, firstColumnHeader = "t/s", columnHeader = "Sec"):
    '''write header line of a data file
    Parameters
    ----------
    fileName : string
        file name
    title : string
        first line to be written
    Nsec : int
        number of columns
    firstColumnHeader : string
        header of the first column
    columnHeader : string
        header of each column. Repeated Nsec times with headers
        example : Nsec = 2, columnHeader = "Sec"
        -> "Sec1\tSec2\t"
    '''
    LogFile = open(fileName, 'w')
    # first line
    LogFile.write(title)
    LogFile.write('\n')
    # column headers
    # first column header
    LogFile.write(firstColumnHeader)
    LogFile.write('\t')

    columnHeaderTemplate = columnHeader+"%d\t"
    # each column
    for i in range(Nsec):
        LogFile.write(columnHeaderTemplate % (i + 1))
    LogFile.write('\n')
    LogFile.close()

def LC_Control(scenario, links_obj, LC_distance):
    '''Apply Lane Change control for specific scenario
    Parameters
    -------------
        scenario: dict
            the scenario dictionary
        links_obj: links object of VISSIM
            the links object
        link_groups: list
            the list of link groups
        ramp_links: list
            the list of links with off ramp
        LC_distance: int 
        the distance of LC controlled section (m)

    '''
    
    link_ID = scenario['link']
    link_ID = 10016
    lanes = scenario['lane']
    link_obj = links_obj.ItemByKey(link_ID)
    for Lane in link_obj.Lanes:
        if Lane.AttValue('Index') in lanes:
            Lane.SetAttValue('BlockedVehClasses', '10,20')

def alineaQ(rmRate, density, Qbound, demand, rho_star, Qlength):
    rmMIN = 100
    rmMAX = 1200
    alpha = 1
    #beta = 1
    rm_den = rmRate - alpha * (density - rho_star)
    #rm_Q = demand + beta * (Qbound - Qlength)
    #return max(rmMIN, min(rmMAX, max(rm_den, rm_Q)))
    return max(rmMIN, min(rmMAX, rm_den))

def vsl_FeedbackLinearization(density, flowSection, rho_star, vsl, section_length, link_groups, err_sum, perturbations, rmRate, rampFlow):
    '''
    density: (list[Nsec]) list of densities in all sections
    vsl: (list[Nsec]) current VSL command in each sections
    section_length: (list[Nsec]) length of each section
    
    '''
    startSection = 0    # startSection: (int) the first section controlled with VSL
    endSection = 5      # endSection: (int) the last section upstream the discharging section. 
    Nsec_controlled = endSection - startSection + 1 
    # Nsec_controlled is the number of sections under control, 
    # including the discharging section! The discharging section is considered as one single section

    w = 30
    # incorporate uncertainties in w
    w = w*(1+perturbations[2])
    
    C = 12000
    # C_d = 7200
    vf = 100
    rho_j = C/vf + C/w
    vslMIN = 20
    vslMIN2 = 70
    vslMAX = 100
    rho_e = [rho_star]*(Nsec_controlled+1)   
        
    rho_true = density[startSection:(endSection+2)]
    q_true = flowSection[(startSection+1):(endSection+3)]
    
    # incorporate measurement uncertainties
    rho = [(1+perturbations[1])*x for x in rho_true]
    q = [(1+perturbations[0])*x for x in q_true]
    
    # get ramp flows 
    onFlow = [0.0] * Nsec_controlled
    offFlow = [0.0] * Nsec_controlled
    for i in range(Nsec_controlled):
        for onramp in link_groups[i + startSection + 1]['ONRAMP']:
            onFlow[i] += rampFlow[onramp]
        for offramp in link_groups[i + startSection + 1]['OFFRAMP']:
            offFlow[i] += rampFlow[offramp]

    # compute errors in densities
    x = np.subtract(rho,rho_e)
    for i in range(len(x)):
        err_sum[i] += x[i]
    
    # we need N+1 measurements to produce N VSL commands
    Lambda1 = [20.0]*(Nsec_controlled)
    Lambda2 = [0.0]*(Nsec_controlled)
    c = [-500.0]*(Nsec_controlled)
    v = [0.0]*(Nsec_controlled)
    qv = [0.0]*(Nsec_controlled)
    for i in range(Nsec_controlled):
        qv[i] = q[i+1] + offFlow[i] - onFlow[i] - Lambda1[i]*x[i+1] - Lambda2[i]*(err_sum[i+1] + c[i])
    
    for i in range(Nsec_controlled):
        if i == 0:
            v[i] = qv[i] * w / (w*rho_j - qv[i])
        elif rho[i] == 0:
            v[i] == vf
        else:
            v[i] = qv[i] / rho[i]

    
    for i in range(Nsec_controlled):
        v[i] = round(v[i] * 0.1) * 10
        if v[i] <= vsl[i + startSection]:
            if i == 0:
                v[i] = max(v[i], vsl[i + startSection] - 10, vslMIN)
            else:
                v[i] = max(v[i], vsl[i + startSection] - 10, vslMIN2)
        else:
            v[i] = min(v[i], vsl[i + startSection] + 10, vslMAX)
        vsl[i + startSection] = v[i]

    return vsl, err_sum






def runSimulation(simulationTime_sec, idxScenario, idxController, randomSeed, folderDir, networkDir, demands, relrampflows, perturbations, sec1len, dLC):
    '''run PTV VISSIM simulation using given arguments
    Parameters
    ----------
        simulationTime_sec : int
            Simulation time (s)
        startTime_sec : int
            Time accident starts (s)
        endTime_sec : int
            Time accident ends (s)
        vehicleInput_veh_hr : int
            vehicle input (veh/hr)
        idxScenario : int
            Index of simulation scenarios
        idxController : int
            Controller Index
        randomSeed : int
            seed for random numbers
        folderDir : string, path
            location of data files
        networkDir : string, path
            location of network file
    '''

    # Link Groups
    link_groups = [
                {'MAINLINE': (1,),            'ONRAMP': (),             'OFFRAMP': (),          'DC': 1, 'VSL': (6,7,8,9,10)},
                {'MAINLINE': (2,),            'ONRAMP': (3,),           'OFFRAMP': (4,),        'DC': 2, 'VSL': (11,12,13,14,15,16)},
                {'MAINLINE': (5,),            'ONRAMP': (6,),           'OFFRAMP': (7,),        'DC': 3, 'VSL': (17,18,19,20,21,22)},
                {'MAINLINE': (8,),            'ONRAMP': (),             'OFFRAMP': (9,),        'DC': 4, 'VSL': (23,24,25,26,27)},
                {'MAINLINE': (10,),           'ONRAMP': (11,),          'OFFRAMP': (),          'DC': 5, 'VSL': (28,29,30,31,32,33)},
                {'MAINLINE': (12,),           'ONRAMP': (14,),          'OFFRAMP': (13,),       'DC': 6, 'VSL': (34,35,36,37,38,39)},
                {'MAINLINE': (15,10016),      'ONRAMP': (16,),          'OFFRAMP': (17,),       'DC': 7, 'VSL': (40,41,42,43,44,45)},
                {'MAINLINE': (18,),           'ONRAMP': (),             'OFFRAMP': (),          'DC': 8, 'VSL': (46,47,48,49,50)},
    ]

    
    ramps = {
                3: {'LINK_GROUP': 2, 'TYPE': 'ON', 'INPUT': 2, 'DC': 9, 'SC': 1, 'QC': 1, 'QLENGTH': 100},
                4: {'LINK_GROUP': 2, 'TYPE': 'OFF', 'DC': 10},
                6: {'LINK_GROUP': 3, 'TYPE': 'ON', 'INPUT': 3, 'DC': 11, 'SC': 2, 'QC': 2, 'QLENGTH': 100},
                7: {'LINK_GROUP': 3, 'TYPE': 'OFF', 'DC': 12},
                9: {'LINK_GROUP': 4, 'TYPE': 'OFF', 'DC': 13},
                11: {'LINK_GROUP': 5, 'TYPE': 'ON', 'INPUT': 4, 'DC': 14, 'SC': 3, 'QC': 3, 'QLENGTH': 100},
                14: {'LINK_GROUP': 6, 'TYPE': 'ON', 'INPUT': 5, 'DC': 16, 'SC': 4, 'QC': 4, 'QLENGTH': 100},
                13: {'LINK_GROUP': 6, 'TYPE': 'OFF', 'DC': 15},
                16: {'LINK_GROUP': 7, 'TYPE': 'ON', 'INPUT': 6, 'DC': 17, 'SC': 5, 'QC': 5, 'QLENGTH': 100},
                17: {'LINK_GROUP': 7, 'TYPE': 'OFF', 'DC': 18},
    }
    
    
    '''setting of scenarios'''
    scenarios = [{'group': 8, 'link': 18, 'lane': (2,3), 'coordinate': 20, 'startTime_sec': 30, 'endTime_sec': 60},
                 {'group': 8, 'link': 18, 'lane': (3,), 'coordinate': 20, 'startTime_sec': 600, 'endTime_sec': 4800},
                 {'group': 8, 'link': 18, 'lane': (2,3), 'coordinate': 20, 'startTime_sec': 600, 'endTime_sec': 4800},
                 {'group': 8, 'link': 18, 'lane': (3,4), 'coordinate': 20, 'startTime_sec': 600, 'endTime_sec': 4800}]


    Nsec = len(link_groups)     # number of sections
    simResolution = 5           # No. of simulation steps per second
    stepTime_sec = 1 / simResolution
    Tdata_sec = 30.0            # Data sampling period
    Tctrl_sec = 30.0            # Control time interval
    

    #   Incident time period
    startTime_sec = scenarios[idxScenario]['startTime_sec']
    endTime_sec = scenarios[idxScenario]['endTime_sec']

    vf = 100 # The free flow speed of highway in km/h
    rho_star = min(demands[0]/vf,72)

    # speed[]: average speed of each section
    # density[]: vehicle density of each section
    # flowSection[]: flow rate of each section
    # flowLane[]: flow rate of each lane at bottleneck section
    # vsl[]: vsl command of each section at current control period, a list of dicts
    # vslOld[]: vsl command of each section at previous control period
    speed = [0.0] * Nsec
    density = [0.0] * Nsec
    flowSection = [0.0] * Nsec
    err_sum = [0.0] * Nsec
    vsl = [vf] * Nsec
    
    rampFlow = {}
    rampQue = {}
    rmRate = {}
    for key in ramps:
        rampFlow[key] = 0.0
        rampQue[key] = 0.0
        rmRate[key] = 0.0


    '''Define log file names'''
    FlowFileName = os.path.join(folderDir, "flow_Log.txt")
    DenFileName = os.path.join(folderDir, "den_Log.txt")
    VSLFileName = os.path.join(folderDir, "vsl_Log.txt")

    
    ProgID = "VISSIM.Vissim.1000"
    
    '''file paths'''
    networkFileName = "Corflo_Roadnet.inpx"
    layoutFileName = "Corflo_Roadnet.layx"
    
    ''' Start VISSIM simulation '''
    ''' COM lines'''  

    try:
        print("Client: Creating a Vissim instance")
        vs = com.Dispatch(ProgID)
    
        print("Client: read network and layout")
        vs.LoadNet(os.path.join(networkDir, networkFileName), 0)
        vs.LoadLayout(os.path.join(networkDir, layoutFileName))
    
        ''' initialize simulations '''
        ''' setting random seed, simulation time, simulation resolution, simulation speed '''
        print("Client: Setting simulations")
        vs.Simulation.SetAttValue('SimRes', simResolution)
        vs.Simulation.SetAttValue('UseMaxSimSpeed', True)        
        vs.Simulation.SetAttValue('RandSeed', randomSeed)
        #vs.Simulation.SetAttValue('SimSpeed', 2.0)
        #vs.Presentation.SetAttValue('RecordAnimations', True)

        # Reference to road network
        net = vs.Net

        ''' Set vehicle input '''
        vehInputs = net.VehicleInputs
        for i in range(vehInputs.Count):
            vehInput = vehInputs.ItemByKey(1+i)
            vehInput.SetAttValue('Volume(1)', demands[i])
        
        ''' Set off-ramp flows'''
        if sum(relrampflows) > 0:
            vehroutes = net.VehicleRoutingDecisionsStatic
            vehroute = vehroutes.ItemByKey(1)
            for i in range(vehroute.VehRoutSta.Count):
                vehroute.VehRoutSta.ItemByKey(i+1).SetAttValue('RelFlow(1)', relrampflows[i])
            
        ''' Set default speed limit and 1st sign location'''
        VSLs = net.DesSpeedDecisions
        
        for VSL in VSLs:
            VSL.SetAttValue("DesSpeedDistr(10)", vf)   # car (km/h)
            VSL.SetAttValue("DesSpeedDistr(20)", vf)   # HGV (km/h)

        pos = 4001 - sec1len*1000
        VSLs.ItemByKey(6).SetAttValue("Pos", pos)
        VSLs.ItemByKey(7).SetAttValue("Pos", pos)
        VSLs.ItemByKey(8).SetAttValue("Pos", pos)
        VSLs.ItemByKey(9).SetAttValue("Pos", pos)
        VSLs.ItemByKey(10).SetAttValue("Pos", pos)

        ''' Get Data collection, link objects, etc '''
        dataCollections = net.DataCollectionMeasurements
        links = net.Links
        signalControllers = net.SignalControllers
        #queueCounters = net.QueueCounters

        # Make sure that all lanes are open      
        link_Nlane = {} 
        link_length = {}  

        for link in links:
            ID = link.AttValue('No')
            link_Nlane[ID] = link.AttValue('NumLanes')
            link_length[ID] = link.AttValue('Length2D') #meter
            # Open all lanes
            for lane in link.Lanes:
                lane.SetAttValue('BlockedVehClasses', '')
                 

        section_length = [0.0] * Nsec # length of each section
        for iSec in range(len(link_groups)): 
            for linkID in link_groups[iSec]['MAINLINE']:
                link = links.ItemByKey(linkID)
                section_length[iSec] += link.AttValue('Length2D') #meter


        busNo = 2 * len(scenarios[idxScenario]['lane']) # No. of buses used to block the lane
        busArray = [0] * busNo

        vehs = net.Vehicles
        
        
        # Construct the dictionary of ramp meters
        meters_obj = {}
        for key in ramps.keys():
            ramp = ramps[key]
            if ramp['TYPE'] == 'ON':
                meters_obj[key] = RM.RampMeter(ramp['LINK_GROUP'], vehInputs.ItemByKey(ramp['INPUT']), dataCollections.ItemByKey(ramp['DC']), signalControllers.ItemByKey(ramp['SC']), 0.0, ramp['QLENGTH'], rho_star)
                if idxController == 1 or idxController == 3:
                    rmRate[key] = max(meters_obj[key].INPUT.AttValue('Volume(1)'), 100)
                else:
                    rmRate[key] = 1800
                meters_obj[key].update_rate(rmRate[key])
                
        
        # start simulation
        print("Client: Starting simulation")
        print("Scenario:", idxScenario)
        print("Controller:", idxController)
        print("Random Seed:", randomSeed)

        currentTime = 0
        
        #vs.Simulation.SetAttValue("SimBreakAt", 200)
        #vs.Simulation.RunContinuous()
        
        '''Link Segment Results'''
        
        while currentTime <= simulationTime_sec:
            # run sigle step of simulation
            vs.Simulation.RunSingleStep()
            currentTime = vs.Simulation.AttValue('SimSec')
            for key in meters_obj:
                meters_obj[key].meter_step(stepTime_sec)

            if 0 == currentTime % Tdata_sec:
                # Get density, flow, speed of each section
                for iSection in range(Nsec):
                    # Flow volume in one sampling period
                    dataCollection = dataCollections.ItemByKey(link_groups[iSection]['DC'])                    
                    flowSection[iSection] = (3600/Tdata_sec)*dataCollection.AttValue('Vehs(Current,Last,All)')

                    # Density and Speed in each section
                    Nveh = 0
                    dist = 0
                    denomenator = 0
                    for linkID in link_groups[iSection]['MAINLINE']:
                        link = links.ItemByKey(linkID)
                        link_vehs = link.Vehs
                        num_vehs = link_vehs.Count
                        sum_speed = 0
                        if num_vehs == 0:
                            v = 0
                        else:
                            for link_veh in link_vehs:
                                sum_speed += link_veh.AttValue('Speed')
                            v = sum_speed / num_vehs
                        Nveh += link_vehs.Count
                        dist += v * link_vehs.Count #total distance traveled by all vehicles in one link
                        denomenator += link_length[linkID]
                    density[iSection] = 1000 * Nveh / denomenator #number of vehicles per km
                    if 0 == Nveh:
                        speed[iSection] = 0
                    else:
                        speed[iSection] = dist / Nveh #km/h

                        
                ''' write log files : flowSection, speed, density '''
                
                denFile = open(DenFileName, "a")
                flowFile = open(FlowFileName, "a")
                
                for i in range(Nsec):
                    denFile.write(str(density[i]) + '\t')
                    flowFile.write(str(flowSection[i]) + '\t')
                
                denFile.write('\n')
                denFile.close()
                flowFile.write('\n')
                flowFile.close()
                
                for key in ramps.keys():
                    dataCollection = dataCollections.ItemByKey(ramps[key]['DC'])
                    rampFlow[key] = (3600/Tdata_sec)*(1+perturbations[3])*dataCollection.AttValue('Vehs(Current,Last,All)')
                    

            if currentTime == startTime_sec:
                # set incident scenario
                i = 0
                for lane in scenarios[idxScenario]['lane']:
                    busArray[i] = vehs.AddVehicleAtLinkPosition(300, scenarios[idxScenario]['link'], lane, scenarios[idxScenario]['coordinate'], 0, 0)
                    busArray[i+1] = vehs.AddVehicleAtLinkPosition(300, scenarios[idxScenario]['link'], lane, scenarios[idxScenario]['coordinate']+20, 0, 0)
                    i += 2
                # Apply Lane change control
                if idxController == 2 or idxController == 3:
                    LC_Control(scenarios[idxScenario], links, dLC)

            if currentTime == endTime_sec:
                # Remove the incident
                for veh in busArray:
                    vehs.RemoveVehicle(veh.AttValue('No'))

                # open all closed lanes
                for link in links:
                    for lane in link.Lanes:
                        lane.SetAttValue('BlockedVehClasses', '')
            
            
            #Compute the VSL command and the RM rate
            if 0 == currentTime % Tctrl_sec:
                
                if idxController == 1 or idxController == 3:
                    for key in meters_obj.keys():
                        rampQue[key] = 0.0
                        ramp = meters_obj[key]
                        rmRate[key] = alineaQ(rmRate[key], density[ramp.LINK_GROUP], rampQue[key], meters_obj[key].INPUT.AttValue('Volume(1)'), meters_obj[key].RHOR, meters_obj[key].QLENGTH)
                        ramp.update_rate(rmRate[key])
                
                if (idxController == 3 or idxController == 2) and (startTime_sec <= currentTime < endTime_sec):
                    vsl, err_sum = vsl_FeedbackLinearization(density, flowSection, rho_star, vsl, section_length, link_groups, err_sum, perturbations, rmRate, rampFlow)
                else:
                    vsl = vf*np.ones(np.shape(vsl))

                #Update VSL command in VISSIM
                for iSec in range(Nsec): 
                    for vslID in link_groups[iSec]["VSL"]:
                        VSL = VSLs.ItemByKey(vslID)
                        VSL.SetAttValue("DesSpeedDistr(10)", vsl[iSec])   # car
                        VSL.SetAttValue("DesSpeedDistr(20)", vsl[iSec])   # HGV

                VSLFile = open(VSLFileName, "a") 
                for i in range(Nsec):
                    VSLFile.write(str(vsl[i]) + '\t')
                VSLFile.write('\n')
                VSLFile.close()


    except pywintypes.com_error as err:
        print("err=", err)

        '''
        Error
        The specified configuration is not defined within VISSIM.
        
        Description
        Some methods for evaulations results need a previously configuration for data collection. 
        The error occurs when requesting results that have not been previously configured.
        For example, using the GetSegmentResult() method of the ILink interface to request
        density results can end up with this error if the density has not been requested within the configuration
        ''' 
        

def main():
    idxScenario = 1 # 0: quickBlock 1: OneLaneClosure 2&3: TwoLaneClosure
    CtrlModes = [3,] # 0: No control  1: RM only  2: VSL + LC  3: VSL + LC + RM
    demands = [7500,900,900,900,400,400] # 1 entrance + 5 ramps
    # 5 offramps + 1 mainline, for mainline input only. Onramp inputs routings are set in VISSIM.
    relrampflows = [0.0,0.0,0.0,0.0,0.0,1.0]
    perturbations = [0.0,0.0,0.3,0.0] # measured flows/measured densities/w/rampflow
    Nsim = 10               # Number of Monte Carlo simulations
    sec1lens = [4.0] * Nsim # Distance btw first two signs, must be in [0 , 4] (km)
    dLC = 800 # LC distance

    simulationTime_sec = 5400 #5400 for sce2

    #nMonteCarlo = 3
    #randomSeeds = calcRandomSeeds(nMonteCarlo)

    # Vehicle Composition ID
    # 1: 10% Trucks    
    #demandComposition = 2
    
    networkDir = os.path.abspath(os.curdir)
    for cmode in CtrlModes:
        folderDir = os.path.join(networkDir, 'MicroResults')
        mkdir(folderDir)
        for sec1len in sec1lens:
            lMC = round(random.uniform(1,100))
            runSimulation(simulationTime_sec, idxScenario, cmode, lMC, folderDir, networkDir, demands, relrampflows, perturbations, sec1len, dLC)
            """Run evaluations in fzpEvaluation.py"""
                    

if __name__ == '__main__':
    main()







    
